#ifndef _KINEMATICS_H_
#define _KINEMATICS_H_

#include "rclcpp/rclcpp.hpp"
#include <chassis_controller/pivots.h>
#include <chassis_controller/pid.h>
#include <chassis_controller/speed_limiter.h>
#include "chassis_controller/hardware_config.h"

namespace chassis_controller
{
  /// @brief 底盘解算的接口类
  class Kinematics
  {
  public:
    explicit Kinematics(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node) : node_(node) {}

    virtual ~Kinematics(){};

    virtual void init(int pivot_num, pid_param steer_pid_param, SpeedLimiter limiter_liner, SpeedLimiter limiter_ang,SpeedLimiter limiter_steer) = 0;

    virtual void updateParam(int pivot_num, pid_param steer_pid_param) = 0;

    virtual void update(std::vector<Spivot_value> &pivot_value,
                        std::vector<Spivot_param> &pivot_param,
                        std::vector<Spivot_setpoint> &pivot_setpoint, const rclcpp::Duration &period,
                        bool ready_move) = 0;

    virtual void getCommand(double vx, double vy, double va) = 0;

    virtual void getVelocity(double vx, double vy, double va) = 0;

    virtual void setCommand(std::vector<Spivot_value> &pivot_value,
                            std::vector<Spivot_param> &pivot_param,
                            std::vector<Spivot_setpoint> &pivot_setpoint,
                            const rclcpp::Duration &period) = 0;

    virtual void setSteerCmdId(uint64_t steer_cmd_id) = 0;

    virtual uint64_t getSteerCmdId() = 0;

    virtual bool getSteeringFlag() = 0;

    virtual bool getSteerErrFlag() = 0;

    virtual bool getBrakerFlag() = 0;

    static Kinematics *create(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, int chassis_type);

    inline auto& get_clock() { return *(node_->get_clock()); }

    std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node_;
  };
}
#endif
